from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
from geometry_msgs.msg import Twist,PoseStamped,TwistStamped,PointStamped
import rclpy
import math
from tf2_ros import TransformListener, TransformStamped,Buffer
from rclpy.node import Node
from nav_msgs.msg import Odometry
from std_msgs.msg import String
import tf2_ros
import tf2_geometry_msgs 
from rclpy.time import Time
chassis_cmd_vel_value=Twist()
class SimpleMove(Node):
    def __init__(self):
        super().__init__('nmsl')
        # 转发
        self.vel_subscription = self.create_subscription(Twist,'cmd_vel',self.vel_callback,10)
        self.vel_subscription
        # 目标位
        self.vel_publisher = self.create_publisher(Twist,'/chassis_info',10)
        timer_period = 0.01  # 20ms = 0.02s
        self.timer = self.create_timer(timer_period, self.timer_callback)
        

    def timer_callback(self):
        chassis_cmd_vel_value.angular.z= float(0)
        self.vel_publisher.publish(chassis_cmd_vel_value)

    def vel_callback(self, msg):
        try:
            transform = self.tf_buffer.lookup_transform(
                "map", "base_link",  # 目标坐标系和源坐标系
                Time())
        # 输出转化矩阵
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):    
            return 0.0,0.0
        cmd_vel=msg
        chassis_cmd_vel_value.linear.x=20*cmd_vel.linear.x
        chassis_cmd_vel_value.linear.y=-20*cmd_vel.linear.y
        self.vel_publisher.publish(chassis_cmd_vel_value)  

   
def main(args=None):
    rclpy.init(args=args)
    Simple_Move = SimpleMove()
    rclpy.spin(Simple_Move)
    Simple_Move.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
